#include <ros/ros.h>
#include <mc_ros/perception_handle_node.h>

PerceptionHandle::PerceptionHandle(){
    lidar_fusion_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/fusion_lidar", 10);
    depth_fusion_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/fusion_depth", 10);
    all_fusion_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/fusion_data", 10);
    all_bev_fusion_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/fusion_bev", 10);

    sensor_config_ptr_ = std::make_shared<SensorConfig>();
    sensor_handle_ptr_ = std::make_shared<SensorHandle>(sensor_config_ptr_);
    loop();
}

// 实时处理
void PerceptionHandle::loop(){
    ros::Rate rate(10);
    while(!ros::isShuttingDown()){
        ros::spinOnce();
        rate.sleep();

        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds; 
        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> fusion_clouds; 
        pcl::PointCloud<pcl::PointXYZ>::Ptr fusion_cloud;
        sensor_msgs::PointCloud2::Ptr pointcloud_msg_ptr;

        // 激光点云数据处理
        clouds = sensor_handle_ptr_->Lidar2DHandle();
        if(!clouds.empty()){
            fusion_cloud = sensor_handle_ptr_->fusionPointCloud(clouds);
            fusion_clouds.emplace_back(fusion_cloud);
            pointcloud_msg_ptr 
                = msg_convert::toPointCloud2(fusion_cloud, "base_link");
            lidar_fusion_pub_.publish(pointcloud_msg_ptr);
        }

        // 深度点云数据处理
        clouds = sensor_handle_ptr_->DepthCameraHandle();
        if(!clouds.empty()){
            fusion_cloud = sensor_handle_ptr_->fusionPointCloud(clouds);
            fusion_clouds.emplace_back(fusion_cloud);
            pointcloud_msg_ptr = msg_convert::toPointCloud2(fusion_cloud, "base_link");
            depth_fusion_pub_.publish(pointcloud_msg_ptr);
        }

        // 所有数据融合
        if(!fusion_clouds.empty()){
            // 发布拼接点云
            fusion_cloud = sensor_handle_ptr_->fusionPointCloud(fusion_clouds);
            pointcloud_msg_ptr 
                = msg_convert::toPointCloud2(fusion_cloud, "base_link");
            all_fusion_pub_.publish(pointcloud_msg_ptr);

            // 发布鸟瞰点云
            sensor_handle_ptr_->projectPointCloud(fusion_cloud);
            pointcloud_msg_ptr 
                = msg_convert::toPointCloud2(fusion_cloud, "base_link");
            all_bev_fusion_pub_.publish(pointcloud_msg_ptr);
        }
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "perception_handle_node");

    string glog_dir = ros::param::param<string>("/glog_dir", "/home/xxty/user_config/logs");
    common::glog::init(glog_dir, "perception_handle_node");

    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    LOG(INFO) << "perception_handle_node start...";

    // tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    // tf2_ros::TransformListener tfListener(tfBuffer);
    // ControllerRosNode controller_ros_node(&tfBuffer);
    PerceptionHandle perception_handle;
    ros::spin();

    common::glog::shutdown();
    return 0;
}